/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "FleetFormationGUIMainFrame.h"



FleetFormationGUIMainFrame::FleetFormationGUIMainFrame( wxWindow* parent )
:
  CSync(1,1),
  MainFrame( parent ),
  iGlDisplay(this->m_panel1, this),
  iFleetController(),
  iMasterPC(NULL),
  iMasterSC(NULL),
  iSlavePC(),
  iSlaveSC(),
  iShowRealPose(false)
{
  iFleetController.SetFleetDrivingStyle(KFleetDriveFormationStaysStatic);
}

void FleetFormationGUIMainFrame::OnSetToFormation( wxCommandEvent& event )
{
  
  iFleetController.RemoveAllSlaves();
  iFleetController.RemoveMasterMachine();
  
  std::map<std::string, SRobotIcon*>::iterator masterRobotIter  =iGlDisplay.GetMasterRobot();
  std::vector<SSlaveMachine> slaveVector;
  iGlDisplay.GetSlaveRobots(&slaveVector,m_radioBtn_RelativeFormation->GetValue());
  dPrint(1,"number of slaves... %d",slaveVector.size());
  if(slaveVector.size() > 0 && masterRobotIter != iRobotMap.end()){

    iFleetController.SetMasterMachine(masterRobotIter->second->iPositionClient,
                                      masterRobotIter->second->iSpeedCtrlClient,
                                      masterRobotIter->first);  

    for(int i = 0; i< slaveVector.size();i++){
      iFleetController.AddSlaveMachine(slaveVector[i].positionClient,
                                       slaveVector[i].speedCtrlClient,
                                       slaveVector[i].relativePosition);
    }
    iFleetController.GoToFormation();
  }
  iGlDisplay.LockGLScreen();
  FollowFleetButton->SetValue(true);
	
}

void FleetFormationGUIMainFrame::OnFollowFleet( wxCommandEvent& event )
{
  dPrint(1,"Onfollow Speed");
  bool isSelected=   event.IsChecked();
  if(isSelected){
    dPrint(1,"Setting start to follow");
    iGlDisplay.LockGLScreen();
                     
    iFleetController.StartToFollow();
  }else{
    dPrint(1,"Setting stop to follow");
    iFleetController.StopToFollow();
    iGlDisplay.UnlockGLScreen();
  }


}

void FleetFormationGUIMainFrame::StaticFormationButton( wxCommandEvent& event )
{
  dPrint(1,"GotStativcFormationEvent");
  iFleetController.SetFleetDrivingStyle(KFleetDriveFormationStaysStatic);
	// TODO: Implement StaticFormationButton
}

void FleetFormationGUIMainFrame::RelativeToMasterHeading( wxCommandEvent& event )
{
  iFleetController.SetFleetDrivingStyle(KFleetDriveFormationRelativeToMasterHeading);
	// TODO: Implement RelativeToMasterHeading
}

void FleetFormationGUIMainFrame::OnShowRealPose( wxCommandEvent& event ) 
{
  iGlDisplay.ShowRealPose(event.IsChecked());
}

bool FleetFormationGUIMainFrame::Initialize(gimi::GIMI *aGimiPtr, int aGimboID, int aReportID)
{
  bool result = true;
  
  // Store for future reference.
  iGimi = aGimiPtr;
  
  // iLoggerGenerator = new LoggerGenerator(iGIMIPtr);
  // Start the Gimbo core.
  CComponent::Start(aGimiPtr, aGimboID, aReportID);

  iMaCICtrlClient =new MaCI::MaCICtrl::CMaCICtrlClient(aGimiPtr);
  
  if (iMaCICtrlClient->Open()!=MaCI::KMaCIOK){
    
    dPrint(1,"Couldn't open MaCICtrl client");
    result = false;
  }
  
  return result;
}
//*****************************************************************************

void FleetFormationGUIMainFrame::OnServiceAssignment(const Gimbo::TServiceAssignmentList &aServiceList)
{
 
  DeleteMachines();
  
  
  dPrint(1,"ON service assignment. size %d, size o fthe robot map %d",aServiceList.size(),iRobotMap.size());
  gim::CSync::Lock();
  
  iServiceList= aServiceList;
  Gimbo::TServiceAssignmentList::size_type index = 0;
  MaCI::Position::CPositionClient *posClient;
  MaCI::SpeedCtrl::CSpeedCtrlClient *speedClient;
  while(index < aServiceList.size()){
    //dPrint(1," %s",aServiceList[index].MaCISL.c_str());

    MaCI::MaCICtrl::SMaCISL maciSL(aServiceList[index].MaCISL);

    //    dPrint(1,"Checking if the machine exists. Size of the map %d",iRobotMap.size());
    //IF THE MACHINE DOES NOT EXIST
    if(iRobotMap.find(maciSL.group) == iRobotMap.end()){
      //dPrint(1,"Machine does not exist. size of the map %d",iRobotMap.size());
      int poseX = iRobotMap.size()* 50 +10;

      int poseY =20;

      /* if(poseX > (int)iWidth-50){
        poseX -= (iWidth -50);
        poseY = 50;
        }*/
      
      
      if(maciSL.interface == "MaCI_Position"){
        dPrint(1,"Creating robot to %d %d. Size of the map is %d",poseX,poseY,iRobotMap.size());
        SRobotIcon *icon = new SRobotIcon(poseX,poseY);
        iRobotMap[maciSL.group] = icon;
        dPrint(1,"Creating new position client");
        posClient = new MaCI::Position::CPositionClient(iGimi,0);
        dPrint(1,"Setting default target");
        posClient->SetDefaultTarget(aServiceList[index].targetID);
        if(posClient->Open() == MaCI::KMaCIOK){
          dPrint(1,"PositionClientOpened");
          //posClient->SetPositionClientCallback(this);
          icon->iPositionClient = posClient;
          iRobotMap[maciSL.group] = icon;
        }else{
          delete(icon);

        }
      }else if(maciSL.interface =="MaCI_SpeedCtrl"){
          dPrint(1,"Creating robot to %d %d. Size of the map is %d",poseX,poseY,iRobotMap.size());
        SRobotIcon *icon = new SRobotIcon(poseX,poseY);
        iRobotMap[maciSL.group] = icon;
        speedClient = new MaCI::SpeedCtrl::CSpeedCtrlClient(iGimi,0);
        speedClient->SetDefaultTarget(aServiceList[index].targetID);
        if(speedClient->Open() == MaCI::KMaCIOK){
          icon->iSpeedCtrlClient = speedClient;
          iRobotMap[maciSL.group] = icon;
        }else{
          delete(icon);
          dPrint(1,"Failed to open speedctrl client");
        }

      }


      //If it exists, check if it has coordinatedrive or position interfaces
    }else{
      //dPrint(1,"Machine already exist.Size of the map %d",iRobotMap.size());
      if(maciSL.interface.compare(std::string("MaCI_Position")) == 0){
        posClient = new MaCI::Position::CPositionClient(iGimi,0);
        posClient->SetDefaultTarget(aServiceList[index].targetID);
        if(posClient->Open() == MaCI::KMaCIOK){
          //          posClient->SetPositionClientCallback(this);
          iRobotMap[maciSL.group]->iPositionClient = posClient;
        }else{
          dPrint(1,"Failed to open position client. ");
        }
      }else if(maciSL.interface.compare(std::string("MaCI_SpeedCtrl")) == 0){
        speedClient = new MaCI::SpeedCtrl::CSpeedCtrlClient(iGimi,0);
        speedClient->SetDefaultTarget(aServiceList[index].targetID);
        if(speedClient->Open() == MaCI::KMaCIOK){
          iRobotMap[maciSL.group]->iSpeedCtrlClient = speedClient;
        }else{
          
          dPrint(1,"Failed to open speedctrl client");
        }
      }
    }



    
    index++;
  }
  dPrint(1,"Adding..");
  dPrint(1,"Adding %d robot",iRobotMap.size());

  Unlock();
  iGlDisplay.SetRobots(&iRobotMap);
  
  


}
void FleetFormationGUIMainFrame::DeleteMachines(){

  iFleetController.RemoveMasterMachine();
  iFleetController.RemoveAllSlaves();


  iSlavePC.clear();
  iSlaveSC.clear();
}
